// -*- c -*-
//Modified 1/3/08 by DPM to incorporate shell for iROBOsim

//allow iROBOinit to be called in non-simulation code:
void iROBOinit (int w){}
void iROBOend(){}
int iROBOtrail;
#define BIGARENA 500
#define BIGEMPTYWORLD 501
#define ARENAWORLD 1
#define EMPTYWORLD 2
#define RULERWORLD 3


//end 1/3/08 modifications

int motor_gain_array[1][3] = {{600, 0, 750}};

/////////////////////////////////////////////////////////////
// Time

/*  returns time since reset or reset_system_time in seconds */
float seconds()
{
    return ((float) mseconds()) / 1000.;
}
void sleep(float seconds)
{
    msleep((long)(seconds * 1000.));
}

void msleep(long msec)
{
    long end_time= mseconds() + msec;
    
    while (1) {
        /* if the following test doesn't execute at least once a second,
    msleep may not halt */
        long done= mseconds()-end_time;
        if (done >= 0L && done <= 1000L) break;
        defer();
    }   
}

/////////////////////////////////////////////////////////////
// Button Functions

// Button bit mask definitions
#define A_BTN   0x0001
#define B_BTN   0x0002
#define RIGHT_BTN  0x0010
#define LEFT_BTN  0x0020
#define UP_BTN   0x0040
#define DOWN_BTN  0x0080
#define R_BTN   0x0100
#define L_BTN   0x0200

// ALL_BTNS is a mask including all the buttons that IC has access to.
// This is equivalent to : (A_BTN | B_BTN | RIGHT_BTN | LEFT_BTN |
// UP_BTN | DOWN_BTN | R_BTN | L_BTN)
#define ALL_BTNS  0x03F3

// The following are aliases to provide a uniform interface across
// disparate targets for choosing and escaping from menus
#define CHOOSE_BTN A_BTN
#define ESCAPE_BTN B_BTN

// button_state returns a bit vector of the state of all the buttons.
// To tell if a particular button is pressed you bitwise and the
// result of this function withe one of the bit mask definitions for
// each button listed above.  A 0 means a given button is pressed, and
// non-zero means that it is not.  The check_button function makes
// this more convenient by doing the and and inverting the result so
// that you get a 1 if pressed and and 0 if not.
int button_state()
{
    return callml(113, 0);
}

int check_button(int button) 
{
    return(!(button_state() & button));
}

int a_button()
{
    return check_button(A_BTN);
}

int b_button()
{
    return check_button(B_BTN);
}

int up_button()
{
    return check_button(UP_BTN);
}

int down_button()
{
    return check_button(DOWN_BTN);
}

int right_button()
{
    return check_button(RIGHT_BTN);
}

int left_button()
{
    return check_button(LEFT_BTN);
}

int r_button()
{
    return check_button(R_BTN);
}

int l_button()
{
    return check_button(L_BTN);
}

int any_button()
{
    return !((button_state() & ALL_BTNS) == ALL_BTNS);
}

// The following are aliases to provide a uniform interface across
// disparate targets for choosing and escaping from menus
int choose_button()
{
    return a_button();
}
int escape_button()
{
    return b_button();
}


// The following functions take a button mask and two button states,
// previous and current, and returns whether the state of the
// requested button did the following:
//   transit:   state changed, either by being hit or released
//   hit:       was not pressed previously, but is currently
//   held:      was pressed during both states
//   released:  was pressed previously, but is not currently

int button_transit(int button, int prev_state, int curr_state)
{
    return ((curr_state ^ prev_state) & button);
}

int button_hit(int button, int prev_state, int curr_state)
{
    return ((~curr_state & prev_state) & button);
}

int button_held(int button, int prev_state, int curr_state)
{
    return (~(curr_state | prev_state) & button);
}

int button_released(int button, int prev_state, int curr_state)
{
    return  (curr_state & ~prev_state) & button;
}


/////////////////////////////////////////////////////////////
// Display Functions

void display_clear() 
{
    callml(120, 0);
}

void display_set_xy(int x, int y) 
{
    callml(221, x, y);
}

void display_get_xy(int *x, int *y) 
{
    callml(222, (int)x, (int)y);
}

void display_buffer_set_xy(int x, int y)
{
    callml(223, x, y);
}

void display_buffer_get_xy(int *x, int *y)
{
    callml(224, (int)x, (int)y);
}

void display_clear_at(int x, int y, int width)
{
    // Clear the part of the display starting at (x,y) for width chars
    callml(323, x, y, width);
}

/////////////////////////////////////////////////////////////
// Tone Functions
void beep()
{
    tone(500., .1);
}

/*  1/2 cycle delay in .5us goes in 0x26 and 0x27  */ 
void tone(float frequency, float length)
{
    set_beeper_pitch(frequency);
    beeper_on();
    sleep(length);
    beeper_off();
}

void beeper_on()
{
    callml(140, 1);
}

void beeper_off()
{
    callml(140, 0);
}

void set_beeper_pitch(float frequency)
{
    callml(141, (int)frequency);
}    

/////////////////////////////////////////////////////////////
// Sensor Functions
int digital(int port)
{
    if(port < 16 && port > 7) return callml(100, (port-8)*2);
    printf("Digital must be between 8 and 15\n");
    return -1;
}

int extra_digital(int port)
{
    if(port < 24 && port > 15) return callml(100, ((port-16)*2)+1);
    printf("Extra digital must be between 16 and 23\n");
    return -1;
}

int analog12(int port)
{
    if(port < 8 && port >= 0) return callml(101, port);
    printf("Analog sensors must be between 0 and 7\n");
    return -1;
}

// 8-bit analog for HB compatibility
int analog(int port)
{
    if(port < 8 && port >= 0) return callml(103, port);
    printf("Analog sensors must be between 0 and 7\n");
    return -1;
}

//simulator noops
void sim_set_analog(int port, int value){}
void sim_set_analog12(int port, int value){}
void sim_set_digital(int port, int value){}

/////////////////////////////////////////////////////////////
// Servo Functions
void enable_servos()
{
    callml(1050, 0);
}

void disable_servos()
{
    callml(1051, 0);
}

void set_servo_position(int servo, int pos)
{
    if(servo < 0 || servo > 3)
      {
        printf("Servo index must be between 0 and 3\n");
        return;
    }
    if (pos < 0 || pos > 255)
      {
        printf("Servo position must be between 0 and 255\n");
        return;
    }
    callml(2050, 3-servo, pos);
}

int get_servo_position(int servo)
{
    if(servo < 0 || servo > 3)
      {
        printf("Servo index must be between 0 and 3\n");
        return -1;
    }
    return callml(1052, 3-servo);
}

/////////////////////////////////////////////////////////////
// Motor Functions

//////////////////////////
// XBC-specific motor functions

// WARNING: These only work with firmware version 1.04 or later

// Recommended default acceleration for motor position and velocity
// commands.  Value used can be changed by calling
// set_motion_acceleration.  The actual default is set by the firmware
// on bootup.  Use get_motion_acceleration to determine what value is
// currenlty being used
#define DEFAULT_MOTION_ACCEL  5000

// Acceleration used by the freeze command
#define FREEZE_ACCEL  5000

// Maximum allowable velocity
#define MAX_VEL   1000

#define BLACK_MOTOR 0
#define SILVER_MOTOR 1
#define WHITE_MOTOR 2

long get_motor_position_counter(int motor)
{
    long m;
    callml(202, motor, (int)&m);
    return m;
}

void set_motor_position_counter(int motor, long value)
{
    callml(203, motor, (int)&value);
}

void clear_motor_position_counter(int motor)
{
    long value=0L;
    callml(203, motor, (int)&value);
}




void mav(int motor, int velocity)
{ move_at_velocity(motor, velocity); }

void move_at_velocity(int motor, int velocity)
{
    /*   *******Allow motors to move at any speed **** 
      if(velocity > MAX_VEL || velocity < -MAX_VEL)
      {
        printf("Velocity must be in range -%d <= v <= %d\n", 
               MAX_VEL, MAX_VEL);        
        return;
    }
*/    callml(208, motor, velocity);
}

// Move motor to goal_pos at given velocity.  The amount actually
// moved will be goal_pos - get_motor_position().
void mtp(int motor, int velocity, long goal_pos)
{ move_to_position(motor, velocity, goal_pos); }

void move_to_position(int motor, int velocity, long goal_pos)
{ 
    if(velocity > MAX_VEL || velocity < -MAX_VEL)
      {
        printf("Velocity must be in range -%d <= v <= %d\n", 
               MAX_VEL, MAX_VEL);        
        return;
    }
    callml(304, motor, velocity, (int)&goal_pos);
}

// Move delta_pos relative to the current position of the motor.  The
// final motor tick will be get_motor_position() at the time of the
// call + delta_pos.
void move_relative_position(int motor, int velocity, long delta_pos)
{ 
    if(velocity > MAX_VEL || velocity < -MAX_VEL)
      {
        printf("Velocity must be in range -%d <= v <= %d\n", 
               MAX_VEL, MAX_VEL);        
        return;
    }
    callml(305, motor, velocity, (int)&delta_pos);
}
void mrp(int motor, int velocity, long delta_pos)
{
    return(move_relative_position(motor, velocity, delta_pos));
}

//Turns off or actively holds the motor in position depending  on the situation -- but it may drift
void freeze(int motor)
{
    setpwm(motor,0); //added to make freeze work if motor was running
    callml(209, motor, FREEZE_ACCEL);
}

void set_pid_gains(int x, int y, int z)
{
    callml(301, x, y, z);
}

//Added Motor Function to support multiple motor types

void set_array_gains(int array_index)
{
    set_pid_gains(motor_gain_array[array_index][0],motor_gain_array[array_index][1],motor_gain_array[array_index][2]);
}

void mtp_x(int motor, int velocity, long goal_pos, int motor_type)
{
    set_array_gains(motor_type);
    mtp(motor, velocity, goal_pos);
}

void mrp_x(int motor, int velocity, long delta_pos, int motor_type)
{
    set_array_gains(motor_type);
    mrp(motor, velocity, delta_pos);
}

void mav_x(int motor, int velocity, int motor_type)
{
    set_array_gains(motor_type);
    mav(motor, velocity);
}

void set_motion_acceleration(int motor, int acceleration)
{
    callml(210, motor, acceleration);
}

int get_motion_acceleration(int motor)
{
    return(callml(161, motor));
}

int get_motor_done(int motor)
{
    return(callml(162,motor));
}

void bmd(int motor)
{
    //loop doing nothing while motor position move is in progress
    while(!(callml(162,motor)));
}

void block_motor_done(int motor)
{
    //loop doing nothing while motor position move is in progress
    while(!(callml(162,motor)));
}

void setpwm(int motor, int pwm)
{
    callml(201, motor, pwm);
}

//////////////////////////
// Classic pwm motor functions, scaled for XBC and Blk gear motors
/***************************************************************** */
/*                                                                */
/* MOTOR PRIMITIVES                                               */
/*                                                                */
/*   fd(n) sets motor n to full on in the green direction    */
/*   bk(n) sets motor n to full on in the red direction      */
/*   motor(n, s) sets motor n on at speed s;               */
/*     s= 100 is full on green,                  */
/*     s= -100 is full on red,                   */
/*     s= 0 is off                               */
/*   off(n)  turns off motor n      */
/*                                                                */
/*   alloff() turns off all motors                      */
/*   ao()  turns off all motors                      */
/*                                                                */
/*                                                                */
/*   motors are numbered 0 through 3.                          */
/***************************************************************** */

void fd(int motor)
{
    //    move_at_velocity(motor, MAX_VEL);
    setpwm(motor,100);
}

void bk(int motor)
{ 
    setpwm(motor,-100);
    //    move_at_velocity(motor, -MAX_VEL);
}

void motor(int port, int speed)
{
    if(speed) speed=_scale_speed(speed);
    setpwm(port,speed);
}

int _scale_speed(int speed)
{
    int dir=1;
    if (speed<0) {
        dir=-1;
        speed=-speed;
    }
    if (speed > 0){
        if (speed > 50)
          speed=(speed+100)/2;
        else {
            if (speed > 20)
              speed=(speed-20)/2+60;
            else speed=3*speed;
        }
    }
    if(speed>100)speed=100;
    speed=speed*dir;
    return (speed);
}



void off(int motor)
{
    setpwm(motor, 0);
}

void alloff()
{
    callml(160, 0);
}

void ao()
{
    callml(160, 0);
}

//////////////////////////
// Encoder functions (HB compatible)
// Since there are 16 digital ports, 
// users can actually have 16 encoders.
// For botball, we chop the top 8 off so
// we can have compatible sensors for all
// boards.
// To enable encoders for all 16 ports,
// make each function body look like this:
// if(port <= 16 && !!port)
// {
//     return callml(<func#>, port);
// }


int enable_encoder(int port)
{
    if(port > 7 && port < 16)
      {
        return callml(170, (port-8)*2);
    }
    printf("Encoder port must be between 8 and 15\n");
    return 0;
}

void disable_encoder(int port)
{
    if(port > 7 && port < 16)
      {
        callml(171, (port-8)*2);
        return;
    }
    printf("Encoder port must be between 8 and 15\n");
}

void reset_encoder(int port)
{
    if(port > 7 && port < 16)
      {
        callml(172, (port-8)*2);
        return;
    }
    printf("Encoder port must be between 8 and 15\n");
}

int read_encoder(int port)
{
    if(port > 7 && port < 16)
      {
        return callml(173, (port-8)*2);
    }
    printf("Encoder port must be between 8 and 15\n");
    return 0;
}

int enable_extra_encoder(int port)
{
    if(port > 15 && port < 24)
      {
        return callml(170, ((port-16)*2)+1);
    }
    printf("Extra encoder port must be between 16 and 23\n");
    
    return 0;
}

void disable_extra_encoder(int port)
{
    if(port > 15 && port < 24)
      {
        callml(171, ((port-16)*2)+1);
        return;
    }
    printf("Extra encoder port must be between 16 and 23\n");
}

void reset_extra_encoder(int port)
{
    if(port > 15 && port < 24)
      {
        callml(172, ((port-16)*2)+1);
        return;
    }
    printf("Extra encoder port must be between 16 and 23\n");
}

int read_extra_encoder(int port)
{
    if(port > 15 && port < 24)
      {
        return callml(173, ((port-16)*2)+1);
    }
    printf("Extra encoder port must be between 16 and 23\n");
    return 0;
}

//////////////////////////
// Sonar functions (HB compatible)
// This is a blocking sonar call,
// just like the handyboard version.
// The port number is the lower of the
// two digital ports used for the sonar
// sensor, and will always be used as the 
// input.
// The call will block until the reply
// happens, or up to 50ms, which ever
// happens first. The callml function
// simply returns the number of 
// microseconds for the round trip of
// the signal. 
// This is passed to the scaling function
// scale_sonar, which then turns the value
// into something similar to the handyboard
// value.

int scale_sonar(int microsecs)
{
    int mm;
    mm=(microsecs/100)*17;
    if(mm>4000 || mm<0) mm=32767;
    return mm;
}

int sonar(int port)
{
    if(port > 7 && port < 16) {
        return scale_sonar(callml(177, ((port-8)*2)));
    }        
    printf("Sonar port must be between 8 and 15\n");
}

//////////////////////////
// Sonar functions (XBC specific)
// This is a bank of non-blocking 
// sonar functions for the XBC.

int set_sonar_port(int port)
{
    if(port > 7 && port < 16)  return callml(174, ((port-8)*2));
    printf("Sonar port must be between 8 and 15\n");
}

void send_sonar_ping(int port)
{
    if(port > 7 && port < 16) callml(175, ((port-8)*2));
    printf("Sonar port must be between 8 and 15\n");
    
}

int get_sonar_ping_time(int port)
{
    if(port > 7 && port < 16) return callml(176, ((port-8)*2));
    printf("Sonar port must be between 8 and 15\n");
}

int get_scaled_sonar_value(int port)
{
    if(port > 7 && port < 16) return scale_sonar(callml(176, ((port-8)*2)));
    printf("Sonar port must be between 8 and 15\n");
}

//////////////////////////
// Battery power functions

float power_level()
{
    float p;
    callml(102, (int)&p);
    return p;
}

//////////////////////////
// Random functions

int random(int x)
{
    return callml(192, x);
}

void seed_random(long seed)
{
    callml(193, (int)&seed);
}

//////////////////////////
// Misc functions

void hog_processor()
{
    callml(190,0);
}

void set_green_led(int value)
{
    callml(195, value);
}

void set_red_led(int value)
{
    callml(194, value);
}

void set_digital_output_value(int port, int value)
{
    if(value < 0) value = 0;
    if(value > 1) value = 1;
    if(port < 16 && port > 7) callml(230, (port-8)*2, value);
    else if(port < 24 && port > 15) callml(230, ((port-16)*2)+1,value);
    else printf("Digital must be between 8 and 23\n");
}

// For emacs to interpret formatting uniformly despite dotfile differences:
//   Local variables:
//    comment-column: 40
//    c-indent-level: 4
//    c-basic-offset: 4
//   End:
